from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
import math


def generate_launch_description():

    # inputJS = Node(
    #    package="input",
    #    executable="joystick",
    #    name="js_input",
    #    output="screen",
    #    parameters=[
    #        {"forward_scale": 10.0 / 180 * math.pi},
    #        {"rotate_scale": -10.0 / 180 * math.pi},
    #    ],
    # )
    
    inputJS = Node(
        package="input",
        executable="joystick",
        name="js_input",
        output="screen",
        parameters=[
            {"forward_scale": 0.2},
            {"rotate_scale": -10.0 / 180 * math.pi},
            {"mode": "speed"},
        ],
    )

    plot = Node(
        package="toolbox",
        executable="plot_runtime",
        name="plot_runtime",
        output="screen",
        parameters=[
            {
                "topics": "torque,rotate_torque,target_angle,angle,target_gyro,gyro,target_gyroz,gyroz,target_speed,speed"
            },
            {"auto_start": True},
        ],
    )

    return LaunchDescription(
        [
            inputJS,
            plot,
        ]
    )
